

#include "gpio.h"
#include "power_ctrl.h"
#include "user_struct.h"
#include "IO_check_handle.h"
#include "uart_comm_ex_api.h"
#include "buzzer_ctrl.h"
#include "BMS_can_msg.h"
#include "system_param.h"
#include "fault_analyse.h"
#include "color_LED.h"

STR_POWER_CTRL powerCtrl;


STR_CHARGE_CTRL cleanChargeHdl;
STR_CHARGE_CTRL serChargeHdl;


STR_CHARGE_CTRL chargeCtrl;


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/
void set_power_off_type(ENUM_POWER_OFF type)
{
	powerCtrl.power_off_type = type;
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void power_off_analyse(void)
{
	switch(powerCtrl.power_off_type)
	{
		case IDLE:
			
			break;
		
		case EXE_NOW:
			powerCtrl.FSM = POWER_OFF_EXE;
			break;
		
		case EXE_CANCEL:
		{
			powerCtrl.power_off_req_cnt = 0;
			powerCtrl.power_off_type = IDLE;
			powerCtrl.FSM = POWER_KEY_DIS;
			break;
		}
	}
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void power_on_off_handle(uint8_t power_key_status, uint16_t run_time_ms)
{
	
	power_off_analyse();
	
	switch(powerCtrl.FSM)
	{
		case POWER_ON_KEY:
		{
			if(power_key_status == 1)
			{
				powerCtrl.power_on_cnt += run_time_ms;
				
				if(powerCtrl.power_on_cnt > POWER_ON_CHECK_TIME)
				{
					powerCtrl.power_on_cnt = 0;
					
					EN_POWER_HOLD();
										
					powerCtrl.FSM = POWER_KEY_DIS;
					
					buzzer_param_set(BUZZER_X1, 2, BEEP_NOTICE);
				}
			}
			
			else
			{
				powerCtrl.power_on_cnt = 0;
			}
		
			break;
		}
		
		case POWER_KEY_DIS:
		{	
			// wait power_on key reless
			if(power_key_status == 0)
			{
				powerCtrl.FSM = POWER_ON_HOLD;
			}
			
			power_off_analyse();
		
			break;
		}
		
		case POWER_ON_HOLD:
		{
			if(power_key_status == 1)
			{
				powerCtrl.power_off_cnt += run_time_ms;
				
				if(powerCtrl.power_off_cnt > POWER_OFF_CHECK_TIME)
				{
					powerCtrl.power_off_cnt = 0;
					
					powerCtrl.FSM = POWER_OFF_REQ;
					
					buzzer_param_set(BUZZER_X1, 2, BEEP_NOTICE);
				}
			}
			else
			{
				powerCtrl.power_off_cnt = 0;
			}
						
			break;
		}
		
		case POWER_OFF_REQ:
		{
			
			// send to nav power off request
			
			powerCtrl.power_off_req_cnt += run_time_ms;
			
			if(powerCtrl.power_off_req_cnt > POWER_REQ_HOLD_TIME)
			{
				powerCtrl.power_off_req_cnt = 0;
				
				powerCtrl.FSM = POWER_OFF_EXE;
			}
			else
			{
				powerCtrl.power_off_cnt_down = POWER_REQ_HOLD_TIME - powerCtrl.power_off_req_cnt;
				
				upload_poweroff_request(powerCtrl.power_off_cnt_down);
			}
						
			break;	
		}

		case POWER_OFF_EXE:
		{						
			DIS_POWER_HOLD();		
			
			break;
		}

		default:
			break;
	}
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

ENUM_POWER_STATUS get_power12V_ultra_status(void)
{
	return powerCtrl.power5V_ultra;
}





/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void power_relay_ctrl(ENUM_POWER_STATUS enable, uint16_t run_time_ms)
{
	switch(powerCtrl.mainRelay_FSM)
	{
		case RELAY_POWER_OFF:
		{	
			DIS_POWER_SWITCH_CLEAN();
			DIS_POWER_HOLD_CLEAN();
			DIS_PRECHARGE_CLEAN();

			if(enable == POWER_ON)
			{
				powerCtrl.relay_delay_cnt += run_time_ms;
				if(powerCtrl.relay_delay_cnt > POWER_RELAY_DELAY)
				{
					powerCtrl.mainRelay_FSM = RELAY_PRECHARGE;
					powerCtrl.relay_delay_cnt = 0;
				}
			}
			else
			{
				powerCtrl.relay_delay_cnt = 0;
			}			
			
			break;
		}
					
		case RELAY_PRECHARGE:
		{
			EN_PRECHARGE_CLEAN();
			
			if(powerCtrl.relay_delay_cnt > POWER_PRECHARGE_TIMEOUT)
			{
				powerCtrl.mainRelay_FSM = RELAY_PRECHARGE_FAULT;
				powerCtrl.relay_delay_cnt = 0;
			}
			
			else if((fabsf(powerCtrl.relay_voltage_V - powerCtrl.DC_voltage) < POWER_PRECHARGE_DIFF) &&
					(powerCtrl.relay_delay_cnt > POWER_PRECHARGE_TIMEHOLD))
			{
				powerCtrl.mainRelay_FSM = RELAY_POWER_ON;
				powerCtrl.relay_delay_cnt = 0;
			}
			else
			{
				powerCtrl.relay_delay_cnt += run_time_ms;
			}
			
			if(enable != POWER_ON)
			{
				powerCtrl.mainRelay_FSM = RELAY_POWER_OFF;
			}
			
			break;
		}
		
		case RELAY_POWER_ON:
		{	
			EN_POWER_SWITCH_CLEAN();
		
			powerCtrl.relay_delay_cnt += run_time_ms;
		
			if(powerCtrl.relay_delay_cnt > POWER_RELAY_DELAY)
			{
				DIS_PRECHARGE_CLEAN();
				powerCtrl.mainRelay_FSM = RELAY_POWER_HOLD;
				powerCtrl.relay_delay_cnt = 0;
				
			}
			
			if(enable != POWER_ON)
			{
				powerCtrl.mainRelay_FSM = RELAY_POWER_OFF;
			}
		
			break;
		}
		
		case RELAY_POWER_HOLD:
		{
			EN_POWER_HOLD_CLEAN();
			
			if(powerCtrl.relay_delay_cnt > POWER_RELAY_DELAY)
			{
				DIS_POWER_SWITCH_CLEAN();
				powerCtrl.relay_delay_cnt = POWER_RELAY_DELAY;
			}
			else
			{
				powerCtrl.relay_delay_cnt += run_time_ms;
			}
			
			if(enable != POWER_ON)
			{
				powerCtrl.mainRelay_FSM = RELAY_POWER_OFF;
			}
			
			break;
		}
		
		case RELAY_POWER_FAULT:		
		case RELAY_PRECHARGE_FAULT:
		default:
		{
			DIS_POWER_SWITCH_CLEAN();
			DIS_POWER_HOLD_CLEAN();
			DIS_PRECHARGE_CLEAN();
			
			powerCtrl.relay_delay_cnt = 0;
			
			break;		
		}
	}	
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/
 
void power_periphepral_ctrl(uint16_t run_time_ms)
{
	if(powerCtrl.FSM == POWER_ON_HOLD)
	{
		
		if(powerCtrl.periphepral_start == 0)
		{
			powerCtrl.power12V_device = POWER_ON;
			powerCtrl.power12V_nav    = POWER_ON;
			powerCtrl.power5V_ultra   = POWER_ON;
			powerCtrl.powerDC_motor   = POWER_ON;		
			powerCtrl.powerDC_service = POWER_ON;
			
			powerCtrl.periphepral_start = 1;
		}
		
		
		power_relay_ctrl(powerCtrl.powerDC_motor, run_time_ms);
		
				
	}
	
	
	
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

ENUM_POWER_RELAY charge_relay_ctrl(STR_CHARGE_CTRL *chargeHdl, uint8_t connect_flg)
{
	
	switch(chargeHdl->FSM)
	{
		case CHARGE_OFF:
			
			chargeHdl->FSM_realy = RELAY_POWER_OFF;
		
			if(connect_flg == 1)
			{
				chargeHdl->FSM = CHARGE_ON;
				chargeHdl->delay_cnt = 0;
			}
			
			break;
		
		case CHARGE_ON:
			
			chargeHdl->FSM_realy = RELAY_POWER_ON;
	
			chargeHdl->delay_cnt++;
			if(chargeHdl->delay_cnt > CHARGE_RELAY_ACT_DELAY)
			{
				chargeHdl->delay_cnt = CHARGE_RELAY_ACT_DELAY;				
				
				if(chargeHdl->bms_charge_status == 0)
				{
					if(chargeHdl->bms_bat_soc > DISCHARGE_SOC_THRESHOL)
					{
						chargeHdl->FSM = CHARGE_FULL;
						chargeHdl->delay_cnt = 0;
					}
					else
					{
						chargeHdl->FSM = CHARGE_INT;
						chargeHdl->delay_cnt = 0;
					}
				}
				else
				{
					chargeHdl->FSM_realy = RELAY_POWER_HOLD;
				}
			}
		
			break;
		
		
		case CHARGE_FULL:
			
			chargeHdl->FSM_realy = RELAY_POWER_OFF;
		
			if(chargeHdl->bms_bat_soc < CHARGE_SOC_THRESHOLD)
			{
				chargeHdl->FSM = CHARGE_ON;
			}
			
			if(connect_flg == 0)
			{
				chargeHdl->FSM = CHARGE_OFF;
			}		
		
			break;
		
		case CHARGE_INT:
			
			chargeHdl->FSM_realy = RELAY_POWER_OFF;
			
			chargeHdl->delay_cnt++;
			if(chargeHdl->delay_cnt > CHARGE_RECOVER_DELAY)
			{
				chargeHdl->FSM = CHARGE_OFF;
			}
		
			break;
	}
	
	
	switch(chargeHdl->FSM_realy)
	{
		case RELAY_POWER_OFF:
			
			chargeHdl->relay_ctrl = RESET;
			chargeHdl->relay_hold = RESET;
		
			break;
		
		case RELAY_POWER_ON:
			
			chargeHdl->relay_ctrl = SET;
			chargeHdl->relay_hold = RESET;			
		
			break;
		
		case RELAY_POWER_HOLD:
			
			chargeHdl->relay_ctrl = SET;
			chargeHdl->relay_hold = SET;	
		
			break;
		
		default:
			break;
	}
	
	return chargeHdl->FSM_realy;
}







/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void power_charge_ctrl(uint16_t run_time_ms)
{
	#if 0
	// clean robot charge ctrl --------------------------
	
	cleanChargeHdl.bms_bat_soc = bmsStatus.raw.bit.soc;
	cleanChargeHdl.bms_charge_status = bmsStatus.raw.bit.charge_FSM;
	
	charge_relay_ctrl(&cleanChargeHdl, userHdl.signal.charge_clean);	
	
	
	// service robot charge ctrl -------------------------
	serChargeHdl.bms_bat_soc = serBmsStatus.soc;
	serChargeHdl.bms_charge_status = serBmsStatus.FSM1.bit.charge_FSM;
	
	charge_relay_ctrl(&serChargeHdl, userHdl.signal.charge_service);	
	
	// battery comm overtime disable charge ------------
	
	if(batCommOTP.fault == 1)
	{
		cleanChargeHdl.FSM = CHARGE_OFF;
		serChargeHdl.FSM = CHARGE_OFF;
	}
	#endif

}




/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void power_manage_handle(uint16_t run_time_ms)
{
	
	power_periphepral_ctrl(run_time_ms);
	
	power_charge_ctrl(run_time_ms);
	
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void auto_charge_ctrl(uint16_t run_time_ms)
{
	static uint16_t relay_cnt = 0;

	switch(chargeCtrl.FSM)
	{
		case CHARGE_OFF:
		{
			userHdl.assis.ledBlkPrd = LED_BLK_STATE_IDLE;	
			userHdl.ctrl.type_LED_left = LED_COLOR_WHITE;
			userHdl.ctrl.type_LED_right = LED_COLOR_WHITE;
			
			if((userHdl.signal.limit_X1 == 1) && (userHdl.signal.limit_X2 == 1))
			{
				relay_cnt += run_time_ms;
				if(relay_cnt > POWER_RELAY_DELAY)
				{
					chargeCtrl.FSM = CHARGE_ON;
				}
			}
			
			chargeCtrl.charge_time_s = 0;
			
			userHdl.ctrl.charge_relay = 0;
			userHdl.ctrl.charge_hold = 0;

			break;
		}
		
		case CHARGE_ON:
		{
			userHdl.assis.ledBlkPrd = LED_BLK_STATE_ONLINE;	
			userHdl.ctrl.type_LED_left = LED_COLOR_GREEN;
			userHdl.ctrl.type_LED_right = LED_COLOR_GREEN;
			
			userHdl.ctrl.charge_relay = 1;
			userHdl.ctrl.charge_hold = 1;
			
			
			chargeCtrl.charge_time_s += ((float32_t)run_time_ms * 0.001f);
			
			if((userHdl.signal.limit_X1 == 0) || (userHdl.signal.limit_X2 == 0))
			{
				chargeCtrl.FSM = CHARGE_OFF;				
			}
			
			break;
		}
		
		case CHARGE_INT:
		{
			userHdl.assis.ledBlkPrd = LED_BLK_STATE_FAULT;	
			userHdl.ctrl.type_LED_left = LED_COLOR_RED;
			userHdl.ctrl.type_LED_right = LED_COLOR_RED;
			
			userHdl.ctrl.charge_relay = 0;
			userHdl.ctrl.charge_hold = 0;
			
			
			break;
		}
		
		default:
			break;
	}	
}



